#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 时间一致的协同制导 2021年1月7日 无人车控制节点
import os
# import cv2
import time
import numpy as np
import math
# from cir2 import findcir
import rospy
from std_msgs.msg import Float32
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
from std_msgs.msg import Int32
from gazebo_msgs.msg import ModelState
from geometry_msgs.msg import Point
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Quaternion
from car_control.msg import LinktrackAnchorframe0
from sensor_msgs.msg import Imu
import time
import json

a_L1 = 1.0
a_L2_x = 1
a_L2_y = 1
a_L3_x = 1
a_L3_y = 1

set_Line1_point = np.array([-1.8, 0, 0])
set_Line1_y = np.array([-1.5, -0.5, 0.5, 1.5]) * a_L1
set_Line1_x = np.array([0, 0, 0, 0])
'''//4//3//2//1//'''
set_Line2_point = np.array([1.5, 0, 0])
set_Line2_y = np.array([-1, -1, 1, 1]) * a_L2_y
set_Line2_x = np.array([-0.3, 0.3, 0.3, -0.3]) * a_L2_x
'''//3//2//'''
'''//4//1//'''
set_Line3_point = np.array([0, 0, 0])
set_Line3_y = np.array([0, -1, 0, 1]) * a_L3_y
set_Line3_x = np.array([-0.3, 0, 0.3, 0]) * a_L3_x
'''////3////'''
'''//4///2//'''
'''////1////'''

f = 20.0
db_K = 1  # mm
vel_Px = 0.5
vel_Py = 0.5
w_P = 0.3
vel_akm = 0.5
w_akm = 0.5
R1 = 1


class xtzd:
    def __init__(self):
        global command
        global car1_position, car1_yaw, car2_position, car2_yaw, car3_position, car3_yaw, car4_position, car4_yaw
        # 设置循环的频率
        rate = rospy.Rate(f)
        command = 0
        t = 0
        rospy.Subscriber('/command', Int32, self.command_callback)
        rospy.Subscriber('/car1/position', Point, self.car1_position_callback)
        rospy.Subscriber('/car2/position', Point, self.car2_position_callback)
        rospy.Subscriber('/car3/position', Point, self.car3_position_callback)
        rospy.Subscriber('/car4/position', Point, self.car4_position_callback)
        rospy.Subscriber('/car1/quaternion', Quaternion, self.car1_quaternion_callback)
        rospy.Subscriber('/car2/quaternion', Quaternion, self.car2_quaternion_callback)
        rospy.Subscriber('/car3/quaternion', Quaternion, self.car3_quaternion_callback)
        rospy.Subscriber('/car4/quaternion', Quaternion, self.car4_quaternion_callback)

        car1_vel, car2_vel, car3_vel, car4_vel = Twist(), Twist(), Twist(), Twist()
        car1_position, car2_position, car3_position, car4_position = [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]
        car1_yaw, car2_yaw, car3_yaw, car4_yaw = 0, 0, 0, 0

        # Main while loop.
        while not rospy.is_shutdown():
            set_point = set_Line1_point
            set_gj_x = set_Line1_x
            set_gj_y = set_Line1_y

            if command == 0:
                t = 0
                car1_exceptyaw = car1_yaw - 90 * math.pi / 180
                car2_exceptyaw = car2_yaw - 90 * math.pi / 180
                car3_exceptyaw = car3_yaw
                car4_exceptyaw = car4_yaw
                print('stop')
                self.stop()

            elif command == 3:
                # set_point = np.array([0, 0.05*t, 0])
                # phi = 2 * math.pi * t / 10 - math.pi / 2 + 10 * math.pi / 180
                # set_gj_x = np.array([R1 * math.cos(phi), R1 * math.cos(phi - math.pi / 2), 0, 0])
                # set_gj_y = np.array([R1 * math.sin(phi), R1 * math.sin(phi - math.pi / 2), 0, 0])
                t1 = t
                t2 = t - 3
                set_gj_x = np.array([self.gj(t1)[0], self.gj(t2)[0], 0, 0])
                set_gj_y = np.array([self.gj(t1)[1], self.gj(t2)[1], 0, 0])

                # car1_exp_pos = self.cal_exp_pos(set_point, [set_gj_x[0], set_gj_y[0], 0])
                car1_exp_pos = [set_gj_x[0], set_gj_y[0], 0]
                car1_vel = self.vel_control(car1_vel, car1_position, car1_exp_pos, car1_yaw, car1_exceptyaw)
                if t2 >= 0:
                    # car2_exp_pos = self.cal_exp_pos(set_point, [set_gj_x[1], set_gj_y[1], 0])
                    car2_exp_pos = [set_gj_x[1], set_gj_y[1], 0]
                    car2_vel = self.vel_control(car2_vel, car2_position, car2_exp_pos, car2_yaw, car2_exceptyaw)
                '''
                car3_exp_pos = self.cal_exp_pos(set_point, [set_gj_x[2], set_gj_y[2], 0])
                car3_vel = self.vel_control(car3_vel, car3_position, car3_exp_pos, car3_yaw, car3_exceptyaw)
                car4_exp_pos = self.cal_exp_pos(set_point, [set_gj_x[3], set_gj_y[3], 0])
                car4_vel = self.vel_control(car4_vel, car4_position, car4_exp_pos, car4_yaw, car4_exceptyaw)
                '''
                print(car1_vel)
                car1_vel_pub.publish(car1_vel)
                car2_vel_pub.publish(car2_vel)
                # car3_vel_pub.publish(car3_vel)
                # car4_vel_pub.publish(car4_vel)
                t = t + 1 / f
            rate.sleep()

    def gj(self, t0):
        x = 0
        y = 0
        T = 8
        t = t0 % (2 * T)

        if 0 <= t <= T / 2:
            midx = -1
            midy = 0
            Rx = 1
            Ry = 1.2
            phi = -2 * math.pi * t / T + math.pi - 10 * math.pi / 180
            x = midx + Rx * math.cos(phi)
            y = midy + Ry * math.sin(phi)
        elif t <= 3 * T / 2:
            midx = 1
            midy = 0
            Rx = 1
            Ry = 1.2
            phi = 2 * math.pi * (t - T / 2) / T + math.pi + 10 * math.pi / 180
            x = midx + Rx * math.cos(phi)
            y = midy + Ry * math.sin(phi)
        elif t <= 2 * T:
            midx = -1
            midy = 0
            Rx = 1
            Ry = 1.2
            phi = -2 * math.pi * (t - 3 * T / 2) / T - 10 * math.pi / 180
            x = midx + Rx * math.cos(phi)
            y = midy + Ry * math.sin(phi)

        return [x, y]

    def cal_exp_pos(self, center_pos, exp_pos):
        x = center_pos[0] + exp_pos[0]
        y = center_pos[1] + exp_pos[1]
        z = center_pos[2] + exp_pos[2]
        return [x, y, z]

    def vel_control(self, vel, now_3d, exp_3d, now_ang, exp_ang):
        # print(exp_3d, now_3d)
        r = math.sqrt(pow(exp_3d[1] - now_3d[1], 2) + pow(exp_3d[0] - now_3d[0], 2))
        q = math.atan2(exp_3d[1] - now_3d[1], exp_3d[0] - now_3d[0]) - (now_ang - exp_ang)

        if q < -math.pi:
            q = q + 2 * math.pi
        elif q > math.pi:
            q = q - 2 * math.pi
        '''
        print('q', math.atan2(exp_3d[1] - now_3d[1], exp_3d[0] - now_3d[0]), (now_ang - exp_ang),
              math.atan2(exp_3d[1] - now_3d[1], exp_3d[0] - now_3d[0]) - (now_ang - exp_ang), q)
        print('pos', exp_3d[0], exp_3d[1], now_3d[0], now_3d[1])
        '''
        # print(r, q)
        vel.linear.x = vel_akm * r
        vel.angular.z = w_akm * q
        return vel

    def command_callback(self, msg):
        global command
        command = msg.data
        # print(command)
        print('Receive command  ', msg.data)

    def car1_position_callback(self, msg):
        global car1_position
        car1_position = self.point2np(msg)

    def car2_position_callback(self, msg):
        global car2_position
        car2_position = self.point2np(msg)

    def car3_position_callback(self, msg):
        global car3_position
        car3_position = self.point2np(msg)

    def car4_position_callback(self, msg):
        global car4_position
        car4_position = self.point2np(msg)

    def car1_quaternion_callback(self, msg):
        global car1_yaw
        (_, _, car1_yaw) = self.quat_to_euler(msg)

    def car2_quaternion_callback(self, msg):
        global car2_yaw
        (_, _, car2_yaw) = self.quat_to_euler(msg)

    def car3_quaternion_callback(self, msg):
        global car3_yaw
        (_, _, car3_yaw) = self.quat_to_euler(msg)

    def car4_quaternion_callback(self, msg):
        global car4_yaw
        (_, _, car4_yaw) = self.quat_to_euler(msg)

    def quat_to_euler(self, orientation):
        x = orientation.x
        y = orientation.y
        z = orientation.z
        w = orientation.w
        r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
        p = math.asin(2 * (w * y - x * z))
        y = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y))
        rpy = [r, p, y]
        return rpy

    def point2np(self, point):
        x = point.x / db_K
        y = point.y / db_K
        z = point.z / db_K
        return [x, y, z]

    def set_pose(self, pose):
        [x, y, z] = pose
        point = Point()
        point.x = x
        point.y = y
        point.z = z

        return point

    def stop(self):
        car1_vel, car2_vel, car3_vel, car4_vel = Twist(), Twist(), Twist(), Twist()
        car1_vel_pub.publish(car1_vel)
        car2_vel_pub.publish(car2_vel)
        car3_vel_pub.publish(car3_vel)
        car4_vel_pub.publish(car4_vel)

    def dis(self, position1, position2):
        dis = math.sqrt(pow(position1[1] - position2[1], 2) + pow(position1[0] - position2[0], 2))
        return dis

    def q_cal(self, position1, position2):
        q = math.atan2((position1[1] - position2[1]), (position1[0] - position2[0]))
        return q

    def satt(self, U):
        if U > U_M:
            Y = U_M
        elif U < -U_M:
            Y = -U_M
        else:
            Y = U
        return Y


if __name__ == '__main__':
    # ROS节点初始化
    rospy.init_node('xtzd', anonymous=True)
    try:
        car1_vel_pub = rospy.Publisher('/car1/cmd_vel', Twist, queue_size=10)
        car2_vel_pub = rospy.Publisher('/car2/cmd_vel', Twist, queue_size=10)
        car3_vel_pub = rospy.Publisher('/car3/cmd_vel', Twist, queue_size=10)
        car4_vel_pub = rospy.Publisher('/car4/cmd_vel', Twist, queue_size=10)
        xtzd()
    except rospy.ROSInterruptException:
        pass
